#pragma once

#include <vector>
#include <cassert>
#include <numeric>
#include <algorithm> // std::min_element
#include <iterator>  // std::begin, std::end

using namespace std;


# define M_PI           3.14159265358979323846  /* pi */



/* Generate 'interp' function */
template<typename T> bool isInClosedRange(T x, T left, T right)
{
	if (x >= left && x <= right)
	{
		return true;
	}
	else
	{
		return false;
	}
}

template<typename T> T interp1(T x, vector<T>vx, vector<T> vy)
{
	/*
	* vx must be monotonically varied vector
	* x must be within vx. If x is outside vx then it will not working
	*/
	T y = 0.0;
	assert(vx.size() > 0 && vx.size() == vy.size() && "interp 1 input is not OK, please check!");
	if (vx.size() == 1)
	{
		return vy[0];
	}
	if (vx.size() > 1)
	{
		T maxX = max(vx[0], vx[vx.size() - 1]);
		T minX = min(vx[0], vx[vx.size() - 1]);
		assert(x >= minX && x <= maxX && "interp1: x is out of range!");
	}
	int n1 = 0, n2 = 1;
	for (int i = 1; i < vx.size(); i++)
	{
		if (isInClosedRange(x, vx[i - 1], vx[i]))
		{
			n1 = i - 1;
			n2 = i;
			break;
		}
	}
	T x1 = vx[n1];
	T x2 = vx[n2];
	T y1 = vy[n1];
	T y2 = vy[n2];
	assert(x2 != x1);
	y = y1 + (y2 - y1) * (x - x1) / (x2 - x1);
	return y;

}

template<typename T> vector<T> interp1(vector<T> x, vector<T>vx, vector<T> vy)
{
	/*
	* vx must be monotonically varied vector
	* x must be within vx. If x is outside vx then it will not working
	*/
	vector<T> y;
	y.resize(x.size());

	assert(vx.size() > 0 && vx.size() == vy.size() && "interp 1 input is not OK, please check!");
	if (vx.size() == 1)
	{
		y = vector<T>(x.size(), vy[0]);

	}
	if (vx.size() > 1)
	{
		T maxX = max(vx[0], vx[vx.size() - 1]);
		T minX = min(vx[0], vx[vx.size() - 1]);
		auto arrayTemp = VectorToArray(x);
		int numberTemp = (arrayTemp < minX && arrayTemp > maxX).count();
		assert(numberTemp <= 0 && "interp1: x is out of range!");
	}
	int n1 = 0, n2 = 1;
	for (int j = 0; j < x.size(); j++)
	{
		for (int i = 1; i < vx.size(); i++)
		{
			if (isInClosedRange(x[j], vx[i - 1], vx[i]))
			{
				n1 = i - 1;
				n2 = i;
				break;
			}
		}
		T x1 = vx[n1];
		T x2 = vx[n2];
		T y1 = vy[n1];
		T y2 = vy[n2];
		assert(x2 != x1);
		y[j] = y1 + (y2 - y1) * (x[j] - x1) / (x2 - x1);
	}

	return y;

}

// Get min position

template<typename T> int getMinPosition(vector<T> v1)
{
	auto n = v1.size();
	auto first = v1.begin();
	auto last = v1.end();
	auto  minIterator = min_element(first, last);
	return (distance(first, minIterator));
}